from src.connectors.lw100_qa_servo_motor.lw_modbus_connector import LwModbusConnector
from src.logger.motor_logger import MotorLogger
from src.utils.lw100_qa_util import Lw100QAUtil


class P100fServoMotorDriver:

    def __init__(self):
        # 日志
        self.logger = MotorLogger()
        self.modbus_connector = LwModbusConnector.get_instance()
        self.modbus_connector.modbus_rtu_client()
        self.modbus_connector.open_client_device()

    def get_driver_number(self, parameter_number: str):
        """
        获取电机编号
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number=parameter_number)
        self.logger.info_log(f"parameter number address is: {rtu_address}")

        # rtu_address = 258
        res = self.modbus_connector.rtu_read_holding_register(rtu_address, 1, 1)
        self.logger.info_log(f"lw_qa servo motor driver number is: {res}")
        return res

    def get_rated_torque(self):
        """
        获取电机的额定转矩
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H00.12")
        self.logger.info_log(f"get rated torque parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_read_holding_register(rtu_address, 1, 1)
        self.logger.info_log(f"rated torque is: {res}")
        return res

    def get_inertia(self):
        """
        获取电机的转动惯量
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H00.16")
        self.logger.info_log(f"get motor inertia parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_read_holding_register(rtu_address, 1, 1)
        self.logger.info_log(f"motor inertia is: {res}")
        return res

    def set_control_mode(self, choice_number: int = 0):
        """
        设置控制模式
        0- 速度模式
        1- 位置模式
        2- 转矩模式
        3- 转矩模式↔速度模式
        4- 速度模式↔位置模式
        5- 转矩模式↔位置模式
        6- 转矩模式↔速度↔位置混合模式
        :return:
        """
        res = self.modbus_connector.rtu_write_single_register(choice_number, 41, 1)
        self.logger.info_log(f"set control mode res: {res}")

    def set_sport_mode(self, choice_number: int):
        """
        设置运动模式
        0- 增量位置模式
        1- 绝对位置线性模式
        2- 绝对位置旋转模式
        :param choice_number:
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H02.01")
        self.logger.info_log(f"set sports mode parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(choice_number, rtu_address, 1)
        self.logger.info_log(f"set sports mode res: {res}")

    def set_rotate_direction(self, choice_number: int):
        """
        设置旋转方向
        0- 以 CCW 方向为正转方向
        (A 超前 B)
        1- 以 CW 方向为正转方向
        ( 反转模式，A 滞后 B)
        :param choice_number:
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H02.02")
        self.logger.info_log(f"set rotate direction parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(choice_number, rtu_address, 1)
        self.logger.info_log(f"set rotate direction res: {res}")

    def position_control_cmd_src(self, choice_number: int = 0):
        """
        位置控制，位置指令来源
        0- 脉冲指令
        1- 步进量给定
        2- 多段位置指令给定
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H05.00")
        self.logger.info_log(f"position control cmd src parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(choice_number, rtu_address, 1)
        self.logger.info_log(f"position control cmd src res: {res}")

    def position_control_pulse_count_percent_circle(self, position_pulse_number: int = 0):
        """
        电机每旋转一圈的位置指令数
        范围：0~1048576
        :param position_pulse_number:
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H05.02")
        self.logger.info_log(f"position control pulse percent circle parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(position_pulse_number, rtu_address, 1)
        self.logger.info_log(f"position control position pulse percent circle res: {res}")

    def position_control_pulse_direction(self, direction_choice_number: int = 0):
        """
        脉冲指令形态
        0- 脉冲 + 方向，正逻辑
        1- 脉冲 + 方向，负逻辑
        2-A 相 +B 相正交脉冲，4 倍频
        3-CW+CCW
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H05.15")
        self.logger.info_log(f"position control pulse direction parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(direction_choice_number, rtu_address, 1)
        self.logger.info_log(f"position control direction res: {res}")

    def position_control_encoder_frequency_division_pulse_number(self, pulse_number: int):
        """
        编码器分频脉冲数
        35 ~32767
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H05.17")
        self.logger.info_log(
            f"position control encoder frequency division pulse number parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(pulse_number, rtu_address, 1)
        self.logger.info_log(f"position control encoder frequency division pulse number res: {res}")

    def speed_control_mode_main_speed_cmd_src(self, cmd_src_number: int = 0):
        """
        速度控制模式主速度指令来源
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H06.00")
        self.logger.info_log(
            f"speed control mode main speed cmd src parameter number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(cmd_src_number, rtu_address, 1)
        self.logger.info_log(f"speed control mode main speed cmd src res: {res}")

    def speed_control_mode_speed_value(self, speed_number: int = 100):
        """
        速度控制模式，速度设定值
        0~6000
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H06.03")
        self.logger.info_log(
            f"speed control mode set speed value number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(speed_number, rtu_address, 1)
        self.logger.info_log(f"speed control mode set speed value res: {res}")

    def speed_control_mode_speed_reach(self, speed_number: int = 100):
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H06.04")
        self.logger.info_log(
            f"speed control mode set speed reach number address is: {rtu_address}")
        res = self.modbus_connector.rtu_write_single_register(speed_number, rtu_address, 1)
        self.logger.info_log(f"speed control mode set speed reach res: {res}")

    def enable_motor_driver(self):
        """
        电机使能
        1-使能
        :return:
        """
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H03.11")
        res = self.modbus_connector.rtu_write_single_register(1, rtu_address, 1)
        self.logger.info_log(f"enable motor res: {res}")

    def disable_motor_driver(self):
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number="H03.11")
        res = self.modbus_connector.rtu_write_single_register(0, rtu_address, 1)
        self.logger.info_log(f"enable motor res: {res}")

    def continuous_rotate(self, parameter_number: str):
        rtu_address = Lw100QAUtil.convert_to_register_address(parameter_number=parameter_number)
        print(f"rtu_address: {rtu_address}")


if __name__ == "__main__":
    lw_servo_driver = P100fServoMotorDriver()
    # lw_servo_driver.get_driver_number(parameter_number="H01.02")
    # lw_servo_driver.enable_motor_driver()
    lw_servo_driver.disable_motor_driver()
    # lw_servo_driver.set_rotate_direction(1)
